function lm_pos = estimate_landmark_position(pose_graph, lm_idx, current_t)
% 从图中估计路标位置
% 输入: pose_graph-位姿图, lm_idx-路标索引, current_t-当前时间步
% 输出: lm_pos-路标估计位置[x; y]

    % 查找所有关于该路标的观测边
    lm_observations = [];
    
    for i = 1:length(pose_graph.edges)
        edge = pose_graph.edges(i);
        if strcmp(edge.type, 'landmark') && edge.landmark_id == lm_idx
            % 获取观测位姿
            pose_id = edge.pose_id;
            if pose_id <= size(pose_graph.poses, 2)
                pose = pose_graph.poses(:, pose_id);
                z = edge.measurement;  % [range; bearing]
                
                % 将观测转换为全局坐标
                range = z(1);
                bearing = z(2);
                lm_x = pose(1) + range * cos(pose(3) + bearing);
                lm_y = pose(2) + range * sin(pose(3) + bearing);
                
                lm_observations = [lm_observations, [lm_x; lm_y]];
            end
        end
    end
    
    % 使用所有观测的平均值作为估计
    if ~isempty(lm_observations)
        lm_pos = mean(lm_observations, 2);
    else
        % 如果没有观测，返回原点（不应该发生）
        lm_pos = [0; 0];
    end
end

